#include "main.h"
#include "stdio.h"
#include "tmc2209.h"


// TMC2209写寄存器函数
void TMC2209::write_register(uint8_t address, uint32_t config)
{
    uint8_t data[5];
    data[0] = (address << 1) & 0xFE; // Write = 0
    data[1] = (config >> 24) & 0xFF;
    data[2] = (config >> 16) & 0xFF;
    data[3] = (config >> 8) & 0xFF;
    data[4] = config & 0xFF;

    HAL_UART_Transmit(p_uart2, data, 5, 100);
}

uint32_t TMC2209::read_register(UART_HandleTypeDef *huart, uint8_t address)
{
    uint8_t data[4];
    uint8_t cmd[5];

    cmd[0] = (address << 1) & 0xFE; // Read = 0
    cmd[1] = cmd[2] = cmd[3] = cmd[4] = 0;

    HAL_UART_Transmit(huart, cmd, 5, 100);
    HAL_UART_Receive(huart, data, 4, 100);

    return ((uint32_t)data[0] << 24) | ((uint32_t)data[1] << 16) | ((uint32_t)data[2] << 8) | (uint32_t)data[3];
}

int TMC2209::check_stallGuard( )
{
    uint32_t drvStatus = read_register(p_uart2, TMC2209_REG_DRV_STATUS);
    int stallGuardValue = drvStatus & 0xFF; // StallGuard2的结果位于最低8位

    if(stallGuardValue < 50) { // 这个阈值可能需要根据实际情况调整
        return -1; // 表示电机堵转
    } else {
        return 0; // 正常运行
    }
}
// 初始化TMC2209：设置微步模式为256
TMC2209::TMC2209(UART_HandleTypeDef *_p_uart2)
{
	p_uart2 = _p_uart2;
	// 设置 CHOPCONF 寄存器：256微步
	write_register(TMC2209_REG_CHOPCONF, 0x00010083);

	// 设置CHOPCONF寄存器以启用StallGuard2
	write_register(TMC2209_REG_CHOPCONF, 0x000100C3); // 示例配置，包含SGTHRS设置

    // 设置 GLOBAL_SCALER：全电流
	write_register(TMC2209_REG_GLOBAL_SCALER, 0x000000FF);

    // 设置 SMARTEN：默认
	write_register(TMC2209_REG_SMARTEN, 0x80050000);

    // 设置 SGCSCONF：电流设置
	write_register(TMC2209_REG_SGCSCONF, 0x00001313);




}
